import rclpy
from rclpy.node import Node
from topic_msg.msg import CatStatus
from topic_msg.msg import CatSteer
from topic_msg.msg import CatDeriction
import threading
import time
import random


# 尾巴：12号，范围45-120（上-下）
class CatTailNode(Node):
    TAIL_ANGLE_MID = 90
    TAIL_ANGLE_MAX = 120
    TAIL_ANGLE_MIN = 45

    def __init__(self, name):
        super().__init__(name)
        self._cat_status_lock = threading.Lock()
        self._cat_status = CatStatus.CAT_ACTIVE
        self._angle = CatTailNode.TAIL_ANGLE_MID
        time.sleep(3)
        self._cat_status_subscriper = self.create_subscription(
            CatStatus, "cat_status", self._rcv_cat_status_callback, 10
        )
        self._cat_deriction = CatDeriction.HORIZONTAL
        self._cat_deriction_lock = threading.Lock()
        self._cat_deriction_subscriper = self.create_subscription(
            CatDeriction, "cat_deriction", self._rcv_cat_deriction_callback, 10
        )

        self._tail_publisher = self.create_publisher(CatSteer, "cat_steers", 10)

        self._set_cat_tail_angle()
        self._thd = threading.Thread(target=self._run)
        self._thd.start()

    def __del__(self):
        self._thd.join()
        self._cat_status_subscriper.destroy()
        self._tail_publisher.destroy()
        self._cat_deriction_subscriper.destroy()

    def _rcv_cat_deriction_callback(self, msg):
        with self._cat_deriction_lock:
            self._cat_deriction = msg.deriction

    def _rcv_cat_status_callback(self, msg):
        with self._cat_status_lock:
            self._cat_status = msg.status

    def _run(self):
        while rclpy.ok():
            time.sleep(1)
            with self._cat_status_lock:
                if (
                    self._cat_status == CatStatus.CAT_STOP
                    or self._cat_status == CatStatus.CAT_DONT_MOVE
                ):
                    if self._angle != CatTailNode.TAIL_ANGLE_MID:
                        self._trail_angle(CatTailNode.TAIL_ANGLE_MID)
                    else:
                        pass
                    continue
                else:
                    pass
            with self._cat_status_lock:
                if self._cat_deriction == CatDeriction.VERTICAL:
                    if self._angle != CatTailNode.TAIL_ANGLE_MID:
                        self._trail_angle(CatTailNode.TAIL_ANGLE_MID)
                    else:
                        pass
                    continue
                else:
                    pass
            self._random_wrag()

    def _random_wrag(self):
        if random.randint(0, 5) != 1:
            return
        count = random.randint(1, 5)
        i = 0
        while i < count:
            i += 1
            destination = random.randint(
                CatTailNode.TAIL_ANGLE_MIN, CatTailNode.TAIL_ANGLE_MAX
            )
            self._trail_angle(destination)

    def _trail_angle(self, destination):

        count = int(abs(self._angle - destination) / 1)

        if count == 0:
            return

        div = (self._angle - destination) / count
        i = 0
        while i < count:
            i += 1
            self._angle = int(self._angle - div)
            self._set_cat_tail_angle()
            # self.get_logger().info(f"angle:{self._angle}")
            time.sleep(0.02)

    def _set_cat_tail_angle(self):
        self._deal_out_range_angle()
        msg = CatSteer()
        msg.number = CatSteer.TAIL_STERR_NUMBER
        msg.type = CatSteer.TAIL_TYPE
        msg.steer.append(CatSteer.TAIL_UP_DOWN)
        msg.angle.append(int(self._angle))
        self._tail_publisher.publish(msg)

    def _deal_out_range_angle(self):
        if self._angle < CatTailNode.TAIL_ANGLE_MIN:
            self._angle = CatTailNode.TAIL_ANGLE_MIN
        elif self._angle > CatTailNode.TAIL_ANGLE_MAX:
            self._angle = CatTailNode.TAIL_ANGLE_MAX

    def _transition_angle(self):
        if self._angle > CatTailNode.TAIL_ANGLE_MID:
            self._angle -= 1
        elif self._angle < CatTailNode.TAIL_ANGLE_MID:
            self._angle += 1


def main():
    rclpy.init()
    node = CatTailNode("cat_tail")
    rclpy.spin(node)
    rclpy.shutdown()
